// Fill out your copyright notice in the Description page of Project Settings.

#include "WorldEntityAxis.h"
//#include "proj/proj.h"
#include <mutex>

class FWorldEntityAxis::Proj
{
	//PJ_CONTEXT*	_pjCtx;
	//PJ*			_pj;
	std::mutex	_mutex;

public:

	Proj(const std::string& proj)
		//: _pjCtx(proj_context_create())
		//, _pj(proj_create(_pjCtx, proj.c_str()))
	{}

	~Proj()
	{
		//if (_pj) proj_destroy(_pj);
		//if (_pjCtx) proj_context_destroy(_pjCtx);
	}

	bool IsValid() const
	{
		//return _pj != nullptr;
		return false;
	}

	glm::dvec3 GeodeticToProj(const glm::dvec3& latlong) const
	{
		std::unique_lock<std::mutex> lock(const_cast<Proj*>(this)->_mutex);
		//PJ_COORD a, b;

		//a = proj_coord(proj_torad(latlong.y), proj_torad(latlong.x), latlong.z, 0);
		//b = proj_trans(_pj, PJ_FWD, a);

		//return glm::dvec3(b.enu.e, b.enu.n, b.enu.u);
		return latlong;
	}

	glm::dvec3 ProjToGeodetic(const glm::dvec3& projected) const
	{
		std::unique_lock<std::mutex> lock(const_cast<Proj*>(this)->_mutex);
		//PJ_COORD a, b;

		//a = proj_coord(projected.x, projected.y, projected.z, 0);
		//b = proj_trans(_pj, PJ_INV, a);

		//return glm::dvec3(proj_todeg(b.lpz.phi), proj_todeg(b.lpz.lam), b.lpz.z);
		return projected;
	}
};

glm::dvec3 FWorldEntityAxis::GeodeticToProj(const glm::dvec3& latlong) const
{
	if (_proj)
	{
		// Project
		glm::dvec3 projected = _proj->GeodeticToProj(latlong);

		// False Origin
		projected -= glm::dvec3(FalseOrigin);

		if (FalseOrigin.w != 0.0)
		{
			double hdg = -FalseOrigin.w;
			glm::dmat3x3 hdgMat(
				std::cos(hdg),	std::sin(hdg),	0.0,
				-std::sin(hdg),	std::cos(hdg),	0.0,
				0.0,			0.0,			1.0
			);
			glm::dvec3 rotated = hdgMat * glm::dvec3(projected.x, projected.y, 1.0);
			projected.x = rotated.x;
			projected.y = rotated.y;
		}
	}
	return glm::dvec3();
}

glm::dvec3 FWorldEntityAxis::ProjToGeodetic(const glm::dvec3& projected) const
{
	if (_proj)
	{
		// False Origin
		glm::dvec3 projected2 = projected;
		if (FalseOrigin.w != 0.0)
		{
			double hdg = FalseOrigin.w;
			glm::dmat3x3 hdgMat(
				std::cos(hdg),	std::sin(hdg),	0.0,
				-std::sin(hdg),	std::cos(hdg),	0.0,
				0.0,			0.0,			1.0
			);
			glm::dvec3 rotated = hdgMat * glm::dvec3(projected.x, projected.y, 1.0);
			projected2.x = rotated.x;
			projected2.y = rotated.y;
		}

		projected2 += glm::dvec3(FalseOrigin);

		// Inverse
		return _proj->ProjToGeodetic(projected2);
	}
	return glm::dvec3();
}

FWorldEntityAxis FWorldEntityAxis::FromOpenDRIVE(const std::string& proj)
{
	FWorldEntityAxis axis;

	//auto projObj = std::make_shared<Proj>(proj);
	//if (projObj && projObj->IsValid())
	//{
	//	axis.SourceType = Type::Proj;
	//	axis._proj = projObj;
	//}
	//else
	{
		axis.SourceType = Type::Unknown;
	}

	return axis;
}

FWorldEntityAxis FWorldEntityAxis::FromDatabase(const std::string& proj)
{
	FWorldEntityAxis axis;

	//auto projObj = std::make_shared<Proj>(proj);
	//if (projObj && projObj->IsValid())
	//{
	//	axis.SourceType = Type::Geodetic;
	//	axis._proj = projObj;
	//}
	//else
	//{
		axis.SourceType = Type::Unknown;
	//}

	return axis;
}

FWorldEntityAxis FWorldEntityAxis::Unknown()
{
	FWorldEntityAxis axis;
	axis.SourceType = Type::Unknown;
	return axis;
}

FWorldEntityAxis FWorldEntityAxis::Inherit()
{
	FWorldEntityAxis axis;
	axis.SourceType = Type::Inherit;
	return axis;
}
